#include "outline/bezier_guide.h"
#include "glib/g2/vector.h"
#include <limits>

namespace outline {

template <typename R>
bool intersect(R & a, 
	g2::basic_vector<R> const & p, g2::basic_vector<R> const & d, 
	g2::basic_vector<R> const & b, g2::basic_vector<R> const & n)
{
	R denom = outer_product(n, d);
	if (fabs(denom) < std::numeric_limits<R>::epsilon())
		return false;
	a = outer_product(n, b - p) / denom;
	return true;
}

template <typename T, typename R>
bool move_pole_near(bpoint & b, point<R> & pole,
					std::vector< point<T> > const & nodes, uint begin, uint end,
					point<R> const & pos, point<R> const & deriv)
{
	typedef R real_type;
	typedef point<real_type> real_point_type;

	real_point_type norm(deriv.y, -deriv.x);

	for (uint i = 0; b >= begin+i || b+i+1 < end; ++i) {
		if (b >= begin+i) { 
			bpoint t = b-i;
			real_type a;
			real_point_type p = nodes[t];
			real_point_type d = real_point_type(nodes[t+1]) - real_point_type(nodes[t]);
			if (intersect<real_type>(a, p, d, pos, norm) && a >= -0.001 && a <= 1.001) {
				pole = p + a * d;
				b = t;
				return i != 0; 
			}
		}
		if (i != 0 && b+i+1 < end) {
			bpoint t = b+i;
			real_type a;
			real_point_type p = nodes[t];
			real_point_type d = real_point_type(nodes[t+1]) - real_point_type(nodes[t]);
			if (intersect<real_type>(a, p, d, pos, norm) && a >= -0.001 && a <= 1.001) {
				pole = p + a * d;
				b = t;
				return true; 
			}
		}
	}
	//this may happen when normal to Bezier never intersects the contour made from nodes
	//assert (false); 
	return false;

/*	bool serious_change = false;
	if (b.pull == to_node) {
		bool may_go_backward = false, may_go_forward = false;
		real_type a_backward, a_forward;
		real_point_type d_backward, d_forward;
		real_point_type pole_backward, pole_forward;
		if (b.i > begin) {
			d_backward = real_point_type(nodes[b.i-1]) - real_point_type(nodes[b.i]);
			may_go_backward = intersect<real_type>(a_backward, 
				nodes[b.i], d_backward, pos, norm)
				&& a_backward > 0;
			if (may_go_backward)
				pole_backward = real_point_type(nodes[b.i]) + a_backward * d_backward;
		}
		if (b.i < end-1) {
			d_forward = real_point_type(nodes[b.i+1]) - real_point_type(nodes[b.i]);
			may_go_forward = intersect<real_type>(a_forward, 
				nodes[b.i], d_forward, pos, norm)
				&& a_forward > 0;
			if (may_go_forward)
				pole_forward = real_point_type(nodes[b.i]) + a_forward * d_forward;
		}
		if (may_go_backward && may_go_forward) {
			//choose the nearest point
			if (square(pole_backward - pos) > square(pole_forward - pos))
				may_go_backward = false;
			else
				may_go_forward = false;
		}
		if (may_go_backward) {
			b.pull = to_segm;
			--b.i;
			serious_change = true;
			//if (a_backward <= 1)
			//	pole = pole_backward;
			//else
			//	pole = real_point_type(nodes[b.i]) + real_type(0.5) * d_backward;
			//return true;
		}
		else if (may_go_forward) {
			b.pull = to_segm;
			serious_change = true;
			//if (a_forward <= 1)
			//	pole = pole_forward;
			//else
			//	pole = real_point_type(nodes[b.i]) + real_type(0.5) * d_forward;
			//return true;
		}
		else
			return false;
	}

	assert (b.pull == to_segm);
	real_type a;
	real_point_type p = nodes[b.i];
	real_point_type d = real_point_type(nodes[b.i+1]) - real_point_type(nodes[b.i]);
	if (intersect<real_type>(a, p, d, pos, norm)) {
		if (a <= 0) {
			b.pull = to_node;
			pole = p;
			return true;
		} else if (a >= 1) {
			b.pull = to_node;
			++b.i;
			pole = p + d;
			return true;
		} else
			pole = p + a * d;
	}
	return serious_change;*/
}

template <typename T, typename R>
R bezier_guide(
	std::vector< point<T> > const & nodes,
	uint begin, uint end,
	uint max_iterations,
	R stop_mean_discrepancy,
	R oversample_factor,
	bool controls_initialized,
	bool poles_initialized,
	bezier<R> & controls,
	std::vector< point<R> > & poles,
	bpoint_vector & bvec
)
{
	typedef point<T> node_type;
	typedef std::vector<node_type> nodes_type;
	typedef R real_type;
	typedef point<real_type> real_point_type;
	typedef bezier<real_type> bezier_type;
	typedef std::vector<real_type> real_vector_type;
	typedef std::vector<real_point_type> real_point_vector_type;
	typedef g2::basic_vector<real_type> vector2;

	assert (end > begin);
	uint size = end - begin;
	assert (size >= 2);
	controls.start() = nodes[begin];
	controls.finish() = nodes[end-1];
	if (!controls_initialized) {
		controls.start_control() = nodes[begin + size/3];
		controls.finish_control() = nodes[begin + size*2/3];
	}

	uint bsize = bpoints_count(size, oversample_factor);
	real_type dt = real_type(1) / (bsize-1);
	if (poles_initialized) {
		assert (bvec.size() == bsize);
		assert (poles.size() == bsize);
	}
	else {
		bvec.resize(bsize);
		poles.resize(bsize);
		//bvec[0].pull = to_node;
		//bvec[0].i = begin;
		bvec[0] = begin;
		poles[0] = nodes[begin];
		//bvec[bsize-1].pull = to_node;
		//bvec[bsize-1].i = end-1;
		bvec[bsize-1] = end-2;
		poles[bsize-1] = nodes[end-1];
		for (uint i = 1; i < bsize-1; ++i) {
			real_type node = begin + (size-1) * (i * dt);
			uint unode = node;
			real_type a = node - unode;
			bpoint & b = bvec[i];
			//b.pull = to_segm;
			//b.i = unode;
			b = unode;
			poles[i] = real_point_type(nodes[unode]) + a * 
				(real_point_type(nodes[unode+1]) - real_point_type(nodes[unode]));
		}
	}

	R chisq = 0;
	//assert (max_iterations > 0);
	for (uint iter = 0; iter < max_iterations; ++iter) 
	{
		chisq = controls.best_fit(poles, dt);

		//controls to parameters
		for (uint i = 1; i < bsize-1; ++i) {
			real_point_type pos, deriv;
			controls.value_n_deriv(i*dt, &pos, &deriv);
			move_pole_near(bvec[i], poles[i], nodes, begin, end, pos, deriv);
		}

		if (chisq <= (bsize-2) * stop_mean_discrepancy)
			return 0; //?chisq, we stop iterations beforehand, may be result be much better, so return is optimistic
	}

	return chisq;
}

#define INSTANTIATE(T,R) \
template R bezier_guide( \
	std::vector< point<T> > const & nodes, \
	uint begin, uint end, \
	uint max_iterations, \
	R stop_mean_discrepancy, \
	R oversample_factor, \
	bool controls_initialized, \
	bool poles_initialized, \
	bezier<R> & controls, \
	std::vector< point<R> > & poles, \
	bpoint_vector & bvec \
);

INSTANTIATE(unsigned short, float)
INSTANTIATE(short, float)
INSTANTIATE(int, float)
INSTANTIATE(float, float)
INSTANTIATE(double, double)

#undef INSTANTIATE

template <typename T, typename R>
R incremental_bezier_guide<T,R>::initial_fit(nodes_type const & nodes, uint begin, uint end)
{
	nodes_ = &nodes;
	begin_ = begin;
	end_ = end;

	uint mi = max_iterations;
	if (mi < 10)
		mi = 10;
	//uint bsize = bpoints_count(end - begin_, oversample_factor);

	return bezier_guide(
		*nodes_, begin_, end,
		mi,
		stop_mean_discrepancy,
		oversample_factor,
		false, false, b,
		poles, bvec
	);
}

template <typename T, typename R>
R incremental_bezier_guide<T,R>::incremental_fit(uint end)
{
	assert(nodes_ != 0);

	end_ = end;
	uint bsize = bpoints_count(end - begin_, oversample_factor);
	//bpoint bp = {to_node, end-1};
	if (bsize == poles.size()) {
		poles.back() = (*nodes_)[end-1];
		//bvec.back() = bp;
		bvec.back() = end-2;
	}
	else while (poles.size() < bsize) {
		poles.push_back((*nodes_)[end-1]);
		//bvec.push_back(bp);
		bvec.push_back(end-2);
	}
	assert (bsize == poles.size());

	return bezier_guide(
		*nodes_, begin_, end,
		max_iterations,
		stop_mean_discrepancy,
		oversample_factor,
		true, true, b,
		poles, bvec
	);
}

template class incremental_bezier_guide<unsigned short>;
template class incremental_bezier_guide<unsigned short, double>;
template class incremental_bezier_guide<short>;
template class incremental_bezier_guide<int>;
template class incremental_bezier_guide<float>;
template class incremental_bezier_guide<double>;

} //namespace outline
